//
// Created by engineai on 2024/07/03.
//

#ifndef ZQ_HUMANOID_CONTACTSPRINGDAMPER_H
#define ZQ_HUMANOID_CONTACTSPRINGDAMPER_H

#include "ContactConstraint.h"
#include "../Dynamics/FloatingBaseModel.h"

/*!
 * Class representing contact dynamics for a FloatingBaseModel
 * This computes external forces to apply bodies which are in contact with the ground
 */
template <typename T>
class ContactSpringDamper : public ContactConstraint<T>
{
public:
    /*!
     * Construct a new contact dynamics model
     * @param model : FloatingBaseModel for the contact dynamics
     */
    ContactSpringDamper(FloatingBaseModel<T> *model)
        : ContactConstraint<T>(model)
    {
        _nGC = CC::_model->_nGroundContact;
        for (size_t i(0); i < _nGC; ++i)
        {
            _tangentialDeflections.push_back(Vec2<T>::Zero());
        }
        deflectionRate.resize(_nGC);
    }

    virtual ~ContactSpringDamper() {}

    /*!
     * Run the ground contact model for a single collision plane on a list of
     * ground contact points. The ground is allowed to deform in the tangential
     * direction, but not the normal direction. Instead, the foot penetrates the ground
     * The ground also "remembers" its deformation between separate contact events.
     * (however it does spring back pretty quickly)
     * @param K Ground stiffness
     * @param D Ground damping
     * @param dt Timestep (used for deflection)
     */
    virtual void UpdateExternalForces(T K, T D, T dt);
    virtual void UpdateQdot(FBModelState<T> &state)
    {
        (void)state; /* Do nothing */
    }

protected:
    size_t _nGC;

    void _groundContactWithOffset(T K, T D);

    vectorAligned<Vec2<T>> deflectionRate;
    vectorAligned<Vec2<T>> _tangentialDeflections;
};
#endif // ZQ_HUMANOID_CONTACTSPRINGDAMPER_H
